/*
  S.M.A.C.K - An operating system kernel
  Copyright (C) 2010,2011 Mattias Holm and Kristian Rietveld
  For licensing and a full list of authors of the kernel, see the files
  COPYING and AUTHORS.
*/

#include <cache.h>
#include <bittools.h>
#include <serial.h>
#include "cpu.h"

void
hw_print_cache_info(void)
{
  write_cssr(0);
  uint32_t csidr_l1 = read_csidr();
  write_cssr(2);
  uint32_t csidr_l2 = read_csidr();

  uint32_t line_size_l1 = (1 << (((csidr_l1 & CSIDR_LNSZ_MASK) >> CSIDR_LNSZ_SHIFT) + 2)) * 4;
  uint32_t num_sets_l1 = ((csidr_l1 & CSIDR_NUMSETS_MASK) >> CSIDR_NUMSETS_SHIFT) + 1;
  uint32_t assoc_l1 = ((csidr_l1 & CSIDR_ASSOC_MASK) >> CSIDR_ASSOC_SHIFT) + 1;
  uint32_t l1_size = assoc_l1 * num_sets_l1 * line_size_l1;

  uint32_t line_size_l2 = (1 << (((csidr_l2 & CSIDR_LNSZ_MASK) >> CSIDR_LNSZ_SHIFT) + 2)) * 4;
  uint32_t num_sets_l2 = ((csidr_l2 & CSIDR_NUMSETS_MASK) >> CSIDR_NUMSETS_SHIFT) + 1;
  uint32_t assoc_l2 = ((csidr_l2 & CSIDR_ASSOC_MASK) >> CSIDR_ASSOC_SHIFT) + 1;
  uint32_t l2_size = assoc_l2 * num_sets_l2 * line_size_l2;


  early_puts("CACHE INFO\r\n");
  early_puts("\tL1: ");
  early_putx(line_size_l1);
  early_puts(" B/line * ");
  early_putx(assoc_l1);
  early_puts(" ways * ");
  early_putx(num_sets_l1);
  early_puts(" sets = ");
  early_putx(l1_size/1024);
  early_puts(" KiB\r\n");
  early_puts("\tL2: ");
  early_putx(line_size_l2);
  early_puts(" B/line * ");
  early_putx(assoc_l2);
  early_puts(" ways * ");
  early_putx(num_sets_l2);
  early_puts(" sets = ");
  early_putx(l2_size/1024);
  early_puts(" KiB\r\n");
}


void
hw_cache_invalidate_all(void)
{
  // L1 cache select
  write_cssr(0);
  uint32_t csidr_l1 = read_csidr();
  write_cssr(2);
  uint32_t csidr_l2 = read_csidr();

  uint32_t num_sets_l1 = ((csidr_l1 & CSIDR_NUMSETS_MASK) >> CSIDR_NUMSETS_SHIFT) + 1;
  uint32_t assoc_l1 = ((csidr_l1 & CSIDR_ASSOC_MASK) >> CSIDR_ASSOC_SHIFT) + 1;
#if 0
  uint32_t line_size_l1 = (1 << (((csidr_l1 & CSIDR_LNSZ_MASK) >> CSIDR_LNSZ_SHIFT) + 2)) * 4; // *4 from words to bytes
  uint32_t l1_size = assoc_l1 * num_sets_l1 * line_size_l1;
#endif


  uint32_t num_sets_l2 = ((csidr_l2 & CSIDR_NUMSETS_MASK) >> CSIDR_NUMSETS_SHIFT) + 1;
  uint32_t assoc_l2 = ((csidr_l2 & CSIDR_ASSOC_MASK) >> CSIDR_ASSOC_SHIFT) + 1;
#if 0
  uint32_t line_size_l2 = (1 << (((csidr_l2 & CSIDR_LNSZ_MASK) >> CSIDR_LNSZ_SHIFT) + 2)) * 4;
  uint32_t l2_size = assoc_l2 * num_sets_l2 * line_size_l2;
#endif

  uint32_t l1_l = (((csidr_l1 & CSIDR_LNSZ_MASK) >> CSIDR_LNSZ_SHIFT) + 2);
  uint32_t l1_a = ctz_32(assoc_l1); // lg assoc

  for (uint32_t way = 0 ; way < assoc_l1 ; way ++) {
    for (uint32_t set = 0 ; set < num_sets_l1 ; set ++) {
      uint32_t set_way_op = (way << (32 - l1_a)) | (set << l1_l) | 0;
      __asm__ volatile (
        "isb\n"
        "mcr p15, 0, %[setway], c7, c6, 2          @ Invalidate line L1 DCACHE\n"
        "dsb\n"
        :
        : [setway] "r" (set_way_op)
        );

    }
  }

  uint32_t l2_l = (((csidr_l2 & CSIDR_LNSZ_MASK) >> CSIDR_LNSZ_SHIFT) + 2);
  uint32_t l2_a = ctz_32(assoc_l2); // lg assoc

  for (uint32_t way = 0 ; way < assoc_l2 ; way ++) {
    for (uint32_t set = 0 ; set < num_sets_l2 ; set ++) {
      uint32_t set_way_op = (way << (32 - l2_a)) | (set << l2_l) | (1 << 1);
      __asm__ volatile (
        "isb\n"
        "mcr p15, 0, %[setway], c7, c6, 2          @ Invalidate line L2 CACHE\n"
        "dsb\n"
        :
        : [setway] "r" (set_way_op)
        );
    }
  }
}

void
hw_cache_clean_all(void)
{
  // L1 cache select
  write_cssr(0);
  uint32_t csidr_l1 = read_csidr();
  write_cssr(2);
  uint32_t csidr_l2 = read_csidr();

  uint32_t num_sets_l1 = ((csidr_l1 & CSIDR_NUMSETS_MASK) >> CSIDR_NUMSETS_SHIFT) + 1;
  uint32_t assoc_l1 = ((csidr_l1 & CSIDR_ASSOC_MASK) >> CSIDR_ASSOC_SHIFT) + 1;
#if 0
  uint32_t line_size_l1 = (1 << (((csidr_l1 & CSIDR_LNSZ_MASK) >> CSIDR_LNSZ_SHIFT) + 2)) * 4; // *4 from words to bytes
  uint32_t l1_size = assoc_l1 * num_sets_l1 * line_size_l1;
#endif


  uint32_t num_sets_l2 = ((csidr_l2 & CSIDR_NUMSETS_MASK) >> CSIDR_NUMSETS_SHIFT) + 1;
  uint32_t assoc_l2 = ((csidr_l2 & CSIDR_ASSOC_MASK) >> CSIDR_ASSOC_SHIFT) + 1;
#if 0
  uint32_t line_size_l2 = (1 << (((csidr_l2 & CSIDR_LNSZ_MASK) >> CSIDR_LNSZ_SHIFT) + 2)) * 4;
  uint32_t l2_size = assoc_l2 * num_sets_l2 * line_size_l2;
#endif

  uint32_t l1_l = (((csidr_l1 & CSIDR_LNSZ_MASK) >> CSIDR_LNSZ_SHIFT) + 2);
  uint32_t l1_a = ctz_32(assoc_l1); // lg assoc

  for (uint32_t way = 0 ; way < assoc_l1 ; way ++) {
    for (uint32_t set = 0 ; set < num_sets_l1 ; set ++) {
      uint32_t set_way_op = (way << (32 - l1_a)) | (set << l1_l) | 0;
      __asm__ volatile (
        "isb\n"
        "mcr p15, 0, %[setway], c7, c10, 2          @ Clean line L1 DCACHE\n"
        "dsb\n"
        :
        : [setway] "r" (set_way_op)
        );

    }
  }

  uint32_t l2_l = (((csidr_l2 & CSIDR_LNSZ_MASK) >> CSIDR_LNSZ_SHIFT) + 2);
  uint32_t l2_a = ctz_32(assoc_l2); // lg assoc

  for (uint32_t way = 0 ; way < assoc_l2 ; way ++) {
    for (uint32_t set = 0 ; set < num_sets_l2 ; set ++) {
      uint32_t set_way_op = (way << (32 - l2_a)) | (set << l2_l) | (1 << 1);
      __asm__ volatile (
        "isb\n"
        "mcr p15, 0, %[setway], c7, c10, 2          @ Clean line L2 CACHE\n"
        "dsb\n"
        :
        : [setway] "r" (set_way_op)
        );
    }
  }
}

void
hw_cache_flush(va_t va, size_t size)
{

}

void
hw_bpc_invalidate(void)
{
  __asm__ volatile (
    "mcr p15, 0, r0, c7, c5, 6\n"
    "dsb\n"
    "isb\n"
    );
}
